#!/usr/bin/env python3
# -*- coding: UTF-8 -*-

import os
import sys

sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))

import rospy
from tools.formation_structure import *
from tools.mathlib import *
from fixedwing_sitl.msg import FWstates, Leaderstates

class VirtualLeader:
    def __init__(self):

        self.planeID = 0
        self.uavID = "plane_0"

        self.fwstates = FWstates()
        self.leaderstates = Leaderstates()
        
        # Subscribers
        self.fw_states_sub = rospy.Subscriber(
            self.uavID + "/fixed_wing_control/fw_state",
            FWstates,
            callback=self.fw_state_callback,
        )

        # Pub
        self.vir_leader_pub = rospy.Publisher(
            self.uavID + "/fixed_wing_control/leader_state",
            Leaderstates,
            queue_size=10
        )

    def set_planeID(self, id: int):
        planeID = id
        if planeID == 0:
            self.uavID = "plane_0"
        elif planeID == 1:
            self.uavID = "plane_1"

    def get_ros_time(self, begin:rospy.Time):
        time_now = rospy.Time.now()
        currTimeSec = time_now.secs - begin.secs
        currTimenSec = time_now.nsecs / 1e9 - begin.nsecs / 1e9
        return currTimeSec + currTimenSec
    
    def fw_state_callback(self, msg:FWstates):
        self.fwstates = msg

    def start(self):
        rospy.init_node("vir_leader_node")
        rospy.loginfo("vir_leader_node started")
        rate = rospy.Rate(10.0)
        begin_time = rospy.Time.now()

        while not rospy.is_shutdown():
            current_time = self.get_ros_time(begin_time)
            # rospy.loginfo("current_time: %f s", current_time)

            self.leaderstates.airspeed = self.fwstates.air_speed
            self.leaderstates.altitude = self.fwstates.altitude
            self.leaderstates.latitude = self.fwstates.latitude
            self.leaderstates.longitude = self.fwstates.longitude
            self.leaderstates.ned_pos_x = self.fwstates.ned_pos_x
            self.leaderstates.ned_pos_y = self.fwstates.ned_pos_y
            self.leaderstates.ned_pos_z = self.fwstates.ned_pos_z
            self.leaderstates.ned_vel_x = self.fwstates.ned_vel_x
            self.leaderstates.ned_vel_y = self.fwstates.ned_vel_y
            self.leaderstates.ned_vel_z = self.fwstates.ned_vel_z
            self.leaderstates.global_vel_x = self.fwstates.global_vel_x
            self.leaderstates.global_vel_y = self.fwstates.global_vel_y
            self.leaderstates.global_vel_z = self.fwstates.global_vel_z

            self.vir_leader_pub.publish(self.leaderstates)

            try:
                rate.sleep()
            except rospy.ROSException as e:
                pass

        rospy.loginfo("vir_sim_leader end")

if __name__ == "__main__":
    virleader = VirtualLeader()
    virleader.start()



